from DebugTrace import DebugTrace
from MotorHandler import MotorHandler
from ThermistorHandler import ThermistorHandler

#Debug Levels:
#
# 5 - extremely verbose
# 4 - ????
# 3 - Breakdown of Gcode/MCode interpretation behaviour
# 2 - some slightly more useful stuff
# 1 - Normal trace level
# 0 - No traces

class GCode(object):
        def __init__(self,debugLevel=1):

                self.originalLine = None

                self.G = None
                self.M = None
                self.T = None
                self.S = None
                self.P = None
                self.X = None
                self.Y = None
                self.Z = None
                self.F = None
                self.R = None
                self.E = None
                self.N = None
                self.checksum = None

                self.trace=DebugTrace("GCode",debugLevel)
                
                self.trace.trace(5,"Created new GCode")
                
        def interpret(self):
                segments = self.originalLine.split()

                #Populate the fields of the GCode object
                for segment in segments:
                        self.trace.trace(5,"Segment: "+segment)
                        #Check for the checksum
                        if ("*" in segment):
                                splitup=segment.split("*")
                                
                                self.checksum=int(splitup[1])
                                segment=splitup[0]
                                self.trace.trace(5,"Found Checksum "+str(self.checksum))
                                
                        if len(segment)>0:        
                                commandType = segment[0]
                                value = segment[1:]
                                
                                self.trace.trace(2,"Command Type: "+commandType+"  Value: "+value)
                                
                                if (commandType == "G"):
                                        self.G = int(value)
                                elif(commandType == "M"):
                                        self.M = int(value)
                                elif (commandType == "T"):
                                        self.T = int(value)
                                elif (commandType == "S"):
                                        self.S = float(value)
                                elif (commandType == "P"):
                                        self.P = float(value)
                                elif (commandType == "X"):
                                        self.X = float(value)
                                elif (commandType == "Y"):
                                        self.Y = float(value)
                                elif (commandType == "Z"):
                                        self.Z = float(value)
                                elif (commandType == "F"):
                                        self.F = float(value)
                                elif (commandType == "R"):
                                        self.R = float(value)
                                elif (commandType == "E"):
                                        self.E = float(value)
                                elif (commandType == "N"):
                                        self.N = int(value)
                                else:
                                        self.trace.trace(1, "Unknown command value "+commandType)

        def validateCheckSum(self):
                if (self.checksum==None):
                        return "Pass"

                cs=0
                command = self.originalLine.split("*")[0]

                for char in command:
                        cs = cs ^ ord(char)
                        self.trace.trace(5,"Cur CS: "+str(cs)+"  char: "+str(char))
                cs = cs & 0xff

                self.trace.trace(2,"Calculated checksum: "+str(cs))
                
                if (cs == self.checksum):
                        return "Pass"
                else:
                        return "Fail"
                


class GCodeParser(object):
        def __init__(self,controller):
                self.trace = DebugTrace("GCParser")
                self.mh = MotorHandler()
                self.th = ThermistorHandler(controller)
                self.th.getThermistors()
                self.controller=controller
                

        #Handle standard GCode commands
        def handleGCommand(self,gcode):

                self.trace.trace(2,"Handling G command "+str(gcode.G))
                retVal=None
                if (gcode.G == 0 or gcode.G == 1):  #*****
                        self.trace.trace(3,"Move")
                        #Tell motor handler to move, passing X,Y,Z,E
                        retVal=self.mh.move(gcode.X,gcode.Y,gcode.Z,gcode.E)                        
                        pass
                elif (gcode.G==28): #*****
                        self.trace.trace(3,"Move to origin")
                        #Tell motor handler to home, passing x,y,z,e values
                        retVal=self.mh.home(gcode.X,gcode.Y,gcode.Z,gcode.E)
                        pass
                elif (gcode.G==20):
                        self.trace.trace(3,"Set units to inches")
                        #Tell motor handler to set distance units to inches
                        retVal=self.mh.setMovementUnits("inch")
                        pass
                elif (gcode.G==21): #*****
                        self.trace.trace(3,"Set units to millimetres")
                        #Tell motor handler to set distance units to inches
                        retVal=self.mh.setMovementUnits("mm")
                        pass
                elif (gcode.G==90): #*****
                        self.trace.trace(3,"Set to absolute positioning")
                        #Tell motor handler to set positioning mode to absolute
                        retVal=self.mh.setPositioningMode("absolute")
                        pass
                elif (gcode.G==91):
                        self.trace.trace(3,"Set to relative positioning")
                        #Tell motor handler to set positioning mode to relative
                        retVal=self.mh.setPositioningMode("relative")
                        pass
                elif (gcode.G==92): #*****
                        self.trace.trace(3,"Set current position")
                        if (gcode.X!=None):
                                self.trace.trace(3,"Current X is now set to "+str(gcode.X))
                                #Tell motor handler that X position is now gcode.X
                        if (gcode.Y!=None):
                                self.trace.trace(3,"Current Y is now set to "+str(gcode.Y))
                                #Tell motor handler that Y position is now gcode.Y
                        if (gcode.Z!=None):
                                self.trace.trace(3,"Current Z is now set to "+str(gcode.Z))
                                #Tell motor handler that Z position is now gcode.Z
                        retVal=self.mh.setPosition(gcode.X,gcode.Y,gcode.Z,gcode.E)
                
                else:
                        self.trace.trace(1, "Unsupported G-Code: G"+str(gcode.G))
                        retVal="!!"

                if retVal==None:
                        retVal="!!"

                return retVal


        #Handle a RepRap defined command
        def handleMCommand(self,gcode):
                retVal=None
                if (gcode.M==0):
                        self.trace.trace(3,"Stop")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==1):
                        self.trace.trace(3,"Sleep")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==17):
                        self.trace.trace(3,"Enable/Power all stepper motors")
                        retVal=self.mh.holdMotors()
                        pass
                elif (gcode.M==18):
                        self.trace.trace(3,"Disable all stepper motors")
                        retVal=self.mh.releaseMotors()
                        pass
                elif (gcode.M==42):
                        self.trace.trace(3,"Stop on material exhausted")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==43):
                        self.trace.trace(3,"Stand by on material exhausted")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==82): #*****
                        self.trace.trace(3,"Set extruder to absolute mode")
                        retVal=self.mh.setExtruderMode("absolute")
                        pass
                elif (gcode.M==83):
                        self.trace.trace(3,"Set extruder to relative mode")
                        retVal=self.mh.setExtruderMode("relative")
                        pass
                elif (gcode.M==84): #*****
                        self.trace.trace(3,"Stop idle hold")
                        retVal=self.mh.releaseMotors()
                        pass
                elif (gcode.M==92):
                        self.trace.trace(3,"Set axis_steps_per_unit")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==98):
                        self.trace.trace(3,"get axis_hysteresis_mm")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==99):
                        self.trace.trace(3,"set axis_hysteresis_mm")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==101):
                        self.trace.trace(3,"Turn extruder 1 on Forward/Undo extruder retraction")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==102):
                        self.trace.trace(3,"Turn extruder 1 on Reverse")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==103):
                        self.trace.trace(3,"Turn all extruders off/Extruder retraction")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==104): #*****
                        self.trace.trace(3,"Set extruder temperature")
                        if (gcode.S!=None):
                                #Tell thermistor handler to set holding temperature on
                                #current toolhead to gcode.S
                                #This should return immediately
                                self.th.setHoldingTempFast("Extruder0",gcode.S) ## Remove Hardcoding
                                retVal="ok" #Temporary
                        else:
                                retVal="!!"
                elif (gcode.M==105):
                        self.trace.trace(3,"Get extruder temperature")
                        bedTemp=self.controller.cachedTemp["Bed"]
                        toolTemp=self.controller.cachedTemp["Extruder0"]
                        retVal="ok T:"+str(toolTemp)+" B:"+str(bedTemp) #Temporary
                        pass
                elif (gcode.M==106): #*****
                        self.trace.trace(3,"Fan on")
                        #Turn the fan on (Who will do this???)
                        self.trace.trace(1,"Temp - Fan now on")
                        retVal="ok"
                        pass
                elif (gcode.M==107): #*****
                        self.trace.trace(3,"Fan off")
                        #Turn the fan off (Who does this???)
                        self.trace.trace(1,"Temp - Fan now off")
                        retVal="ok"
                        pass
                elif (gcode.M==108):
                        self.trace.trace(3,"Set extruder speed")
                        retVal="ok"
                        pass
                elif (gcode.M==109): #*****
                        self.trace.trace(3,"Set extruder temperature and wait")
                        if (gcode.S!=None):
                                #Tell thermistor handler to set holding temperature on
                                #current toolhead to gcode.S
                                #This should wait until up to heat to return
                                self.th.setHoldingTempWait("Extruder0",gcode.S) ##Remove hardcoding of Extruder0
                                retVal="ok" #Temporary
                        else:
                                retVal="!!"
                elif (gcode.M==110):
                        self.trace.trace(3,"Set current line number")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==112):
                        self.trace.trace(3,"Emergency Stop")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==113):
                        self.trace.trace(3,"Set extruder PWM")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==114):
                        self.trace.trace(3,"Get current position")
                        retVal="ok C: X:10 Y:10 Z:10 E:10"
                        pass
                elif (gcode.M==115):
                        self.trace.trace(3,"Get firmware version and capabilities")
                        #This is just an example from the GCode page
                        retVal="ok PROTOCOL_VERSION:0.1 FIRMWARE_NAME:FiveD FIRMWARE_URL:http%3A//reprap.org MACHINE_TYPE:Mendel EXTRUDER_COUNT:1"
                        pass
                elif (gcode.M==116):
                        self.trace.trace(3,"Wait")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==117):
                        self.trace.trace(3,"Get Zero Position")
                        retVal="ok C: X:0 Y:0 Z:0 E:0" #Temporary
                        pass
                elif (gcode.M==118):
                        self.trace.trace(3,"Negotiate Features")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==119):
                        self.trace.trace(3,"Get Endstop Status")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==135):
                        self.trace.trace(3,"Set heater output")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==140): #*****
                        self.trace.trace(3,"Bed Temperature (Fast)")
                        if (gcode.S!=None):
                                #Tell thermistor handler to set holding temperature on
                                #print bed to gcode.S
                                #This should return immediately
                                self.th.setBedTempFast(gcode.S)
                                retVal="ok" #Temporary
                        else:
                                retVal="!!"
                elif (gcode.M==143):
                        self.trace.trace(3,"Maximum Hot-End temperature")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==190): #*****
                        self.trace.trace(3,"Wait for bed temperature to reach target temp")
                        if (gcode.S!=None):
                                #Tell thermistor handler to hold control of this process
                                #until print bed reaches temperature gcode.S
                                self.th.waitBedTemp(gcode.S)
                                retVal="ok"#Temporary
                        else:
                                retVal="!!"
                elif (gcode.M==200):
                        self.trace.trace(3,"Set filament diameter/Get endstop status")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==201):
                        self.trace.trace(3,"Set max printing acceleration")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==202):
                        self.trace.trace(3,"Set max travel acceleration")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==203):
                        self.trace.trace(3,"Set maximum feedrate")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==206):
                        self.trace.trace(3,"Set default acceleration")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==207):
                        self.trace.trace(3,"Set home offset")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==208):
                        self.trace.trace(3,"Calibrate Z axis by detecting Z max length")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==209):
                        self.trace.trace(3,"Set axis max travel")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==226):
                        self.trace.trace(3,"GCode initiated pause")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==227 or gcode.M==229):
                        self.trace.trace(3,"Enable automatic reverse and prime")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==228):
                        self.trace.trace(3,"Disable automatic reverse and prime")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==230):
                        self.trace.trace(3,"Disable/Enable wait for temperature change")
                        retVal="ok" #Temporary
                        pass
                elif (gcode.M==300):
                        self.trace.trace(3,"Play beep sound")
                        if (gcode.S !=None and gcode.P !=None):
                                self.trace.trace(3,str(gcode.S)+"Hz  "+str(gcode.P)+"ms")
                                #Beep
                        retVal="ok"
                else:
                        self.trace.trace(1, "Unsupported M-Code: M"+str(gcode.M))
                        retVal="!!"

                return retVal

        #Select a specific toolhead
        def handleTCommand(self,gcode):
                self.trace.trace(3, "Selecting Tool "+str(gcode.T))
                self.mh.setTool(gcode.T)
                self.th.setTool(gcode.T)
                return "ok"

        def parse(self,gcodeLine):
                self.trace.trace(2, gcodeLine)

                #Comments in GCode start with a semicolon, don't parse those
                if (not gcodeLine.startswith(";") and gcodeLine.strip()!=""):
                        gcode=GCode(self.trace.debugLevel)

                        #Segment GCode
                        gcode.originalLine=gcodeLine.split(";")[0] #Only take code from the left of semicolon
                        gcode.interpret()

                        #Make sure any present checksum is valid
                        if (gcode.validateCheckSum() == "Fail"):
                                self.trace.trace(1, "Invalid checksum in line")
                                if (gcode.N != None):
                                        self.trace.trace(1,"Return to line "+str(gcode.N))
                                        return "!!"
                                else:
                                        return "!!"

                        # Two main types of commands, G and M.  G are standard GCodes,
                        # M commands are defined for the RepRap project.  T codes are
                        # used to select tools.  Need to make sure there is only one
                        # of these types of commands per line
                        majorCommandType = None;
                        if (gcode.G != None):
                                majorCommandType = "G"
                        elif (gcode.M != None and majorCommandType == None):
                                majorCommandType = "M"
                        elif (gcode.T != None and majorCommandType == None):
                                majorCommandType = "T"
                        else:
                                if (majorCommandType==None):
                                        self.trace.trace(1, "Invalid GCode line.  No command type")
                                else:
                                        self.trace.trace(1, "Invalid GCode line.  Multiple major command types")

                                return "Fail"



                        #Now pass off the gcode to have its particular details handled
                        retVal=None
                        if (majorCommandType == "G"):
                                retVal=self.handleGCommand(gcode)
                        elif (majorCommandType == "M"):
                                retVal=self.handleMCommand(gcode)
                        elif (majorCommandType == "T"):
                                retVal=self.handleTCommand(gcode)
                        else:
                                self.trace.trace(1, "No major command type found")
                                retVal="!!"
                        return retVal
                else:        
                        return "ok"





